#include "app_motor.h"
#include "system_cfg.h"
#include <string.h>
#include "cJSON.h"
#include "usart.h"

/**
 * 现在所用的直流减速电机的型号为:
 * 单向脉冲编码器
 * 减速比：1:48
 * 空载转速：200RPM
 * 编码器线数：11线
 * 编码器精度：528线
 * 
 * 小车直径65mm，一圈的距离C = π * d = 3.14 * 65 mm = 204mm ，转一圈的脉冲为528个，一个脉冲距离为204/528 = 0.387mm
 * 编码器捕获采用双边沿计数，编码器精度528线，读取脉冲计数为528 * 2 = 1056 个脉冲
 * 调速周期为20ms一次，1秒调速50次，读取码盘的周期为20ms，注意全速下，码盘脉冲周期为300us左右，20ms最多计数140次左右
 * 最小速度设置为:200mm/s 表示20ms周期计数34个脉冲左右;最大速度设置为:1400mm/s 表示20ms周期计数140个脉冲
 * 速度值作为参考，实际速度有偏差，低速下，电机扭矩偏小，容易堵转，速度步增值为10
 */

__IO uint32_t left_encoder_temp, right_encoder_temp; // 左电机和右电机的编码器脉冲计数值 - 临时值
//
uint16_t L_Motor_PWM, R_Motor_PWM;
uint16_t L_Motor_ENC, R_Motor_ENC;
//左右电机的PID参数结构体定义
TYPEDEF_PID_STRUCT gL_MotorPID;	
TYPEDEF_PID_STRUCT gR_MotorPID;	
//PID参数初始化
void PID_init(void)
{	
	//左电机
	gL_MotorPID.SetSpeed = 0.0;
	gL_MotorPID.ActualSpeed = 0.0;
	gL_MotorPID.err = 0.0;
	gL_MotorPID.err_next = 0.0;
	gL_MotorPID.err_last = 0.0;
	gL_MotorPID.Kp = 1.0;
	gL_MotorPID.Ki = 1.0;
	gL_MotorPID.Kd = 0.0;
	//右电机
	gR_MotorPID.SetSpeed = 0.0;
	gR_MotorPID.ActualSpeed = 0.0;
	gR_MotorPID.err = 0.0;
	gR_MotorPID.err_next = 0.0;
	gR_MotorPID.err_last = 0.0;
	gR_MotorPID.Kp = 1.0;
	gR_MotorPID.Ki = 1.0;
	gR_MotorPID.Kd = 0.0;
}
//增量式PID控制算法
float PID_Control_Algorithm(float TargetSpeed, float ActualSpeed, TYPEDEF_PID_STRUCT *PID)
{
	float incrementSpeed;
	//设置速度
	PID->SetSpeed = TargetSpeed;
	PID->ActualSpeed = ActualSpeed;
	//计算误差
	PID->err = PID->SetSpeed - PID->ActualSpeed ;
	//算法实现
	incrementSpeed = PID->Kp * (PID->err - PID->err_next) + PID->Ki * PID->err + PID->Kd * (PID->err - 2 * PID->err_next + PID->err_last);
	PID->ActualSpeed += incrementSpeed;
	//保存误差
	PID->err_last = PID->err_next ;
	PID->err_next = PID->err ;
	//PWM输出设置
	if(PID->ActualSpeed < 200) PID->ActualSpeed = 200;
	if(PID->ActualSpeed > 1000) PID->ActualSpeed = 1000;
	//返回数据
	return PID->ActualSpeed;
}
/*
 * 定时器的输入捕获上升沿触发的中断回调函数
 *
 */
void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim)
{
  /* 左电机编码器 */
  if ((htim->Instance == TIM1) && (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1))
  {
    left_encoder_temp++;
    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1);
  }

  /* 右电机编码器 */
  if ((htim->Instance == TIM14) && (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1))
  {
    right_encoder_temp++;
    HAL_TIM_IC_Start_IT(&htim14, TIM_CHANNEL_1);
  }
}
/**
 * @brief  直流电机编码器的初始化
 * @note
 * @retval None
 */
void motor_encoder_init(void)
{
  /* 清零计数器 */
    __HAL_TIM_SET_COUNTER(&htim1, 0);
    __HAL_TIM_SET_COUNTER(&htim14, 0);

    /* 使能输入捕获中断 */
    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_1);
    HAL_TIM_IC_Start_IT(&htim14, TIM_CHANNEL_1);
}
/**
 * @brief  读取左轮编码器
 * @note
 * @retval
 */
uint32_t motor_read_encoder(te_L_R_Motor_t motor)
{
  uint32_t temp;
  switch (motor)
  {
  case L_MOTOR:
    temp = left_encoder_temp;
    left_encoder_temp = 0;
    break;
  case R_MOTOR:
    temp = right_encoder_temp;
    right_encoder_temp = 0;
    break;

  default:
    temp = 0;
    break;
  }

  return temp;
}

/**
 * @brief  解析JSON数据包
 * @note
 * @param  *pstr:
 * @retval None
 */
void parse_json_data(uint8_t *pstr, uint16_t len)
{
  if (pstr == NULL || len < 2)
    return;

  cJSON *json_root = cJSON_Parse((const char *)pstr);
  if (json_root)
  {
    cJSON *json_control = cJSON_GetObjectItem(json_root, "control");
    if (json_control)
    {
      cJSON *json_turn = cJSON_GetObjectItem(json_control, "turn");
      if (json_turn)
      {
        if (!strcmp(json_turn->valuestring, "run"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_RUN;
		  systemValue.car_mode   = CAR_MODE_MANUAL;
        }
        else if (!strcmp(json_turn->valuestring, "back"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_BACK;
		  systemValue.car_mode   = CAR_MODE_MANUAL;
        }
        else if (!strcmp(json_turn->valuestring, "left"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_LEFT;
		  systemValue.car_mode   = CAR_MODE_MANUAL;
        }
        else if (!strcmp(json_turn->valuestring, "right"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_RIGHT;
		  systemValue.car_mode   = CAR_MODE_MANUAL;
        }
		else if (!strcmp(json_turn->valuestring, "stop"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_STOP;
		  systemValue.car_mode   = CAR_MODE_MANUAL;
        }
		else if (!strcmp(json_turn->valuestring, "auto_track"))
        {
          console_log_success("control turn %s", json_turn->valuestring);
          systemValue.car_status = CAR_STATUS_OFF;
		  systemValue.car_mode   = CAR_MODE_AUTO_TRACK;
        }
      }
      json_turn = NULL;

      cJSON *json_power = cJSON_GetObjectItem(json_control, "power");
      if (json_power)
      {
        if (!strcmp(json_power->valuestring, "on"))
        {
          console_log_success("control power %s", json_power->valuestring);
          systemValue.car_status = CAR_STATUS_ON;
        }
        else if (!strcmp(json_power->valuestring, "off"))
        {
          console_log_success("control power %s", json_power->valuestring);
          systemValue.car_status = CAR_STATUS_OFF;
        }
      }
      json_power = NULL;

      cJSON *json_pwm = cJSON_GetObjectItem(json_control, "pwm");
      if (json_pwm)
      {
        cJSON *json_L_pwm = cJSON_GetObjectItem(json_pwm, "L_Motor");
        cJSON *json_R_pwm = cJSON_GetObjectItem(json_pwm, "R_Motor");
				//保存设置的目标速度，低速下发值：40，中速：70  高速100，Hi3861端下发的PWM值增加了死区时间
//				if((json_L_pwm->valueint) == 390)	//低速350
//				{
//					gL_MotorPID.SetSpeed = (json_L_pwm->valueint - 355) * 10;	//占空比范围：300-1000，电机死区为300左右
//					gR_MotorPID.SetSpeed = (json_R_pwm->valueint - 355) * 10;	//占空比范围：300-1000，电机死区为300左右
//					console_log_success("control pwm_L: %d, pwm_R: %d", L_Motor_PWM, R_Motor_PWM);
//				}
//				else if((json_L_pwm->valueint) == 420)	//中速500
//				{
//					gL_MotorPID.SetSpeed = (json_L_pwm->valueint - 370) * 10;	//占空比范围：300-1000，电机死区为300左右
//					gR_MotorPID.SetSpeed = (json_R_pwm->valueint - 370) * 10;	//占空比范围：300-1000，电机死区为300左右
//					console_log_success("control pwm_L: %d, pwm_R: %d", L_Motor_PWM, R_Motor_PWM);
//				}
//				else if((json_L_pwm->valueint) == 450)	//高速600
//				{
//					gL_MotorPID.SetSpeed = (json_L_pwm->valueint - 390) * 10;	//占空比范围：300-1000，电机死区为300左右
//					gR_MotorPID.SetSpeed = (json_R_pwm->valueint - 390) * 10;	//占空比范围：300-1000，电机死区为300左右
//					console_log_success("control pwm_L: %d, pwm_R: %d", L_Motor_PWM, R_Motor_PWM);
//				}
			if((json_L_pwm->valueint >= 350) && (json_L_pwm->valueint <= 950))	
			{
				gL_MotorPID.SetSpeed = json_L_pwm->valueint;	//占空比范围：300-1000，电机死区为300左右
				gR_MotorPID.SetSpeed = json_R_pwm->valueint;	//占空比范围：300-1000，电机死区为300左右
				console_log_success("control pwm_L: %d, pwm_R: %d", L_Motor_PWM, R_Motor_PWM);
			}
      }
      json_pwm = NULL;
			
    }
    json_control = NULL;
  }
  cJSON_Delete(json_root);
  json_root = NULL;
}




/**
 * @brief  生成JSON数据
 * @note   注意：生成JSON数据完成之后，一定要将packet_buff进行清空处理
 * @param  *sysData:
 * @retval
 */
void packet_json_data(ts_SystemValue_t *sysData)
{
  uint8_t packet_buff[125] = {0};
	//ActualSpeed为实际速度，* 0.968是因微信小程序将 速度值 * 1000 / 1968 * 0.2
  sprintf((char *)packet_buff, "{\"status\":{\"distance\":%d,\"carPower\":%d,\"L_speed\":%d,\"R_speed\":%d}}",
          sysData->distance, sysData->battery, (uint16_t)(gL_MotorPID.ActualSpeed * 0.968), (uint16_t)(gL_MotorPID.ActualSpeed * 0.968));

  HAL_UART_Transmit(&huart1, packet_buff, strlen((char *)packet_buff), 10); // 将数据发送出去
//	console_log_info("buff: %s", packet_buff);
}

// 10ms产生一个中断
volatile uint16_t TIM3_Times_Count = 0;
//
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim->Instance == TIM3) 
  {
		TIM3_Times_Count++;
		if(!(TIM3_Times_Count % 2))
		{
			TIM3_Times_Count = 0;
			//读取编码器值
			systemValue.L_enc = motor_read_encoder(L_MOTOR);
			systemValue.R_enc = motor_read_encoder(R_MOTOR);
			//设置输出PWM
			motor_control_running_status(systemValue.car_status);
		}
  }
}
//
volatile uint8_t gMotorStatus = 0;	//PWM输出是否开启
extern volatile uint8_t gLineOut;	//光电管输出
extern volatile uint8_t gLineType;	//巡线的类型
//
void motor_control_running_status(te_car_status_t status)
{
	L_Motor_ENC = systemValue.L_enc;
	R_Motor_ENC = systemValue.R_enc;
	//通过码盘输出，计算输出的PWM值，关联关系：编码器值*7后与PWM值进行关联，速度步增值为10
	L_Motor_PWM = (uint16_t) PID_Control_Algorithm(gL_MotorPID.SetSpeed, L_Motor_ENC * 7, &gL_MotorPID);
	R_Motor_PWM = (uint16_t) PID_Control_Algorithm(gR_MotorPID.SetSpeed, R_Motor_ENC * 7, &gR_MotorPID);
	//设置输出
  switch (status)
  {
    case CAR_STATUS_RUN:	//前进
      Motor_turn_run(L_Motor_PWM, R_Motor_PWM);
    break;

    case CAR_STATUS_BACK:	//后退
      Motor_turn_back(L_Motor_PWM, R_Motor_PWM);
    break;

    case CAR_STATUS_LEFT:	//左转
			Motor_turn_left(350, 350);
    break;

    case CAR_STATUS_RIGHT:	//右转
			Motor_turn_right(350, 350);
    break;

    case CAR_STATUS_ON:
			Motor_turn_on();	//开启PWM输出
    break;

    case CAR_STATUS_OFF:	

		  //小车在CAR_STATUS_OFF状态与gLineType高四位全为高的情况下进入巡线模式
			if(((gLineType >> 4) == 0x0F) || (systemValue.car_mode == CAR_MODE_AUTO_TRACK))	//巡线模式
			{
				Car_search_black_Line();
			}
			else	//非巡线模式
			{
				//速度值归零
				gL_MotorPID.ActualSpeed = 0.0;
				gR_MotorPID.ActualSpeed = 0.0;	
				gMotorStatus = 0x00;	//巡线模式下，gMotorStatus用于初始化PWM输出
				Motor_turn_off();	//关闭PWM输出
			}
			
    break;
	
	  case CAR_STATUS_STOP:	//PWM输出为零
			//速度值归零
			gL_MotorPID.ActualSpeed = 0.0;
			gR_MotorPID.ActualSpeed = 0.0;
			//
			Motor_turn_stop();
		break;
		
    default:
    break;
  }
}
//巡线模式，黑色非反光胶带，宽度20mm，白色背景，巡线指示灯为绿色，非白色背景，巡线指示灯黄色、红色或者白色
void Car_search_black_Line(void)
{

	//进入巡线模式，先开启PWM输出
	if(gMotorStatus == 0x00)	//未初始化PWM输出
	{
		gMotorStatus = 0xFF;
		Motor_turn_on();	//开启PWM输出
	}
	//根据gLineOut信号，调节左右电机转速
  switch (gLineOut)
  {
    case 0xFE:	//偏左，右电机提速
			L_Motor_PWM = 0; 	//加快响应
			gL_MotorPID.SetSpeed = 0; 
			gR_MotorPID.SetSpeed = 220; 
    break;
    case 0xFD:	//偏左，右电机提速
			L_Motor_PWM = 0; 
			gL_MotorPID.SetSpeed = 0; 
			gR_MotorPID.SetSpeed = 220; 
    break;
    case 0xFB:	//偏左，右电机提速
			gL_MotorPID.SetSpeed = 220; 
			gR_MotorPID.SetSpeed = 260; 
    break;
    case 0xE7:	//中间，速度值相等
			gL_MotorPID.SetSpeed = 260; 
			gR_MotorPID.SetSpeed = 260; 
    break;
    case 0xDF:	//偏右，左电机提速
			gL_MotorPID.SetSpeed = 260; 
			gR_MotorPID.SetSpeed = 220; 
    break;
    case 0xBF:	//偏右，左电机提速
			gL_MotorPID.SetSpeed = 220; 
		  gR_MotorPID.SetSpeed = 0; 
			R_Motor_PWM = 0;	//加快响应
    break;
    case 0x7F:	//偏右，左电机提速
			gL_MotorPID.SetSpeed = 220; 
		  gR_MotorPID.SetSpeed = 0; 
			R_Motor_PWM = 0;	//加快响应
    break;
    default:
    break;
  }
	//电机输出
	if(systemValue.distance <= 150)	//距离小于150mm
	{
		Motor_turn_stop();
		gL_MotorPID.SetSpeed = 0; 
		gR_MotorPID.SetSpeed = 0; 
	}
	else	//前方安全
	{
		Motor_turn_run(L_Motor_PWM, R_Motor_PWM);
	}
	
}
//

/**
 * @brief  停止
 * @note
 * @retval None
 */
void Motor_turn_stop(void)
{
  L_MOTOR_IN0(GPIO_PIN_RESET);
  L_MOTOR_IN1(GPIO_PIN_RESET);
  R_MOTOR_IN0(GPIO_PIN_RESET);
  R_MOTOR_IN1(GPIO_PIN_RESET);
  L_MOTOR_PWM_OUT = R_MOTOR_PWM_OUT = 0;
  L_Motor_PWM = R_Motor_PWM = 0;
	systemValue.L_enc = systemValue.R_enc = 0;
}

/**
 * @brief  左转
 * @note
 * @param  l_pwm:
 * @param  r_pwm:
 * @retval None
 */
void Motor_turn_left(uint16_t l_pwm, uint16_t r_pwm)
{
  L_MOTOR_IN0(GPIO_PIN_SET);
  L_MOTOR_IN1(GPIO_PIN_RESET);
  R_MOTOR_IN0(GPIO_PIN_RESET);
  R_MOTOR_IN1(GPIO_PIN_SET);

  L_MOTOR_PWM_OUT = l_pwm;
  R_MOTOR_PWM_OUT = r_pwm;
}
/**
 * @brief  右转
 * @note
 * @param  l_pwm:
 * @param  r_pwm:
 * @retval None
 */
void Motor_turn_right(uint16_t l_pwm, uint16_t r_pwm)
{
  L_MOTOR_IN0(GPIO_PIN_RESET);
  L_MOTOR_IN1(GPIO_PIN_SET);
  R_MOTOR_IN0(GPIO_PIN_SET);
  R_MOTOR_IN1(GPIO_PIN_RESET);

  L_MOTOR_PWM_OUT = l_pwm;
  R_MOTOR_PWM_OUT = r_pwm;
}
/**
 * @brief  前进
 * @note
 * @param  l_pwm:
 * @param  r_pwm:
 * @retval None
 */
void Motor_turn_run(uint16_t l_pwm, uint16_t r_pwm)
{
  L_MOTOR_IN0(GPIO_PIN_SET);
  L_MOTOR_IN1(GPIO_PIN_RESET);
  R_MOTOR_IN0(GPIO_PIN_SET);
  R_MOTOR_IN1(GPIO_PIN_RESET);

  L_MOTOR_PWM_OUT = l_pwm;
  R_MOTOR_PWM_OUT = r_pwm;
}
/**
 * @brief  后退
 * @note
 * @param  l_pwm:
 * @param  r_pwm:
 * @retval None
 */
void Motor_turn_back(uint16_t l_pwm, uint16_t r_pwm)
{
  L_MOTOR_IN0(GPIO_PIN_RESET);
  L_MOTOR_IN1(GPIO_PIN_SET);
  R_MOTOR_IN0(GPIO_PIN_RESET);
  R_MOTOR_IN1(GPIO_PIN_SET);

  L_MOTOR_PWM_OUT = l_pwm;
  R_MOTOR_PWM_OUT = r_pwm;
}
/**
 * @brief  开启电机
 * @note
 * @retval None
 */
void Motor_turn_on(void)
{
  L_MOTOR_IN0(GPIO_PIN_SET);
  L_MOTOR_IN1(GPIO_PIN_SET);
  R_MOTOR_IN0(GPIO_PIN_SET);
  R_MOTOR_IN1(GPIO_PIN_SET);
  L_MOTOR_PWM_OUT = 0;
  R_MOTOR_PWM_OUT = 0;
	L_Motor_PWM = R_Motor_PWM = 0;
//	systemValue.L_enc = systemValue.R_enc = 0;
  HAL_TIM_PWM_Start(&htim16, TIM_CHANNEL_1); // 电机1
  HAL_TIM_PWM_Start(&htim17, TIM_CHANNEL_1); // 电机2
}
/**
 * @brief  关闭电机
 * @note
 * @retval None
 */
void Motor_turn_off(void)
{
  L_MOTOR_IN0(GPIO_PIN_RESET);
  L_MOTOR_IN1(GPIO_PIN_RESET);
  R_MOTOR_IN0(GPIO_PIN_RESET);
  R_MOTOR_IN1(GPIO_PIN_RESET);
  L_MOTOR_PWM_OUT = R_MOTOR_PWM_OUT = 0;
  L_Motor_PWM = R_Motor_PWM = 0;
//	systemValue.L_enc = systemValue.R_enc = 0;
  HAL_TIM_PWM_Stop(&htim16, TIM_CHANNEL_1); // 电机1
  HAL_TIM_PWM_Stop(&htim17, TIM_CHANNEL_1); // 电机2
}


